﻿#include "LaneKeeping.h"
#include "Types.h"
#include "TrajectoryPool.h"
#include "LaneChanging.h"
#include <iostream>

using namespace std;

using Eigen::VectorXd;
using Eigen::MatrixXd;

std::tuple<VehicleState*, TrajectoryPtr> LineKeeping::OptimalTrajectory( const Eigen::VectorXd& currStateX6,
																		 double currTime,
																		 const SensorFusion& rSF )
{
	bool Keep_current_lane = true;
	bool Change_lane_to_left = true;
	bool Change_lane_to_right = true;

	// Start state: [s0, s0_d, s0_dd] at t0
	// End state:   [s1_d, s1_dd] at t1 = t0 + T
	// st_d = target velocity.
	// Task: generate optimal longitudinal trajectory set of quartic polynomials by varying the end constraints  // 四阶
	const double maxT = 10;
	const double LaneChangingTime = 4;
	const double currentS = currStateX6(0);
	const double currentD = currStateX6(3);
	const int nCurrentLane = Utils::DtoLaneNumber(currentD);
	// trajectorypool: speed_limit and time_horizon
	TrajectoryPool pool (m_SpeedLimit, m_HorizontPrediction);
	TOtherCarsTrajectory CarsOnThisLaneTrajectories = rSF.GetLeadingCarsTrajectoryInLane(currStateX6, 
														nCurrentLane, m_HorizontPrediction, m_TimeStep);

	// Generates trajectory which are staying in current lane
	if (Keep_current_lane)
	{
		pool.SetOtherCars(CarsOnThisLaneTrajectories);
		// 对小于speed_limit的速度采样，然后对时长采样
		for (double v = 0; v < m_SpeedLimit; v += 1)
		{
			for (double T = 1; T < maxT; T += 1)
			{
				// Generate_longitudinal_and_lateral_trajectory 这个函数是固定的，不同的是初始和结尾的运动学状态
				pool.AddTrajectory(Trajectory::Generate_longitudinal_and_lateral_trajectory(currStateX6, 
																		currentD, v, currTime, T, 0));
			}
		}
	}

	// Changing lane to right trajectory
	if (nCurrentLane <= 1 && Change_lane_to_right)
	{
		const int nTargetLane = nCurrentLane + 1;
		const double targetD = Utils::LaneNumberToD(nTargetLane);
		// 加入当前车道的前方其他车辆
		pool.SetOtherCars(CarsOnThisLaneTrajectories);
		// 加入要变道的目标车道的其他车辆
		pool.AddOtherCars(rSF.GetOtherCarsTrajectoryInLane(currStateX6, nTargetLane, m_HorizontPrediction, m_TimeStep));
		for (double v = 0; v < m_SpeedLimit; v += 1)
		{
			for (double T = 1; T < maxT; T += 1)
			{
				TrajectoryPtr pTraj = Trajectory::Generate_longitudinal_and_lateral_trajectory(currStateX6, targetD, v, currTime, T, LaneChangingTime);
				pool.AddTrajectory(pTraj);
			}
		}
	}

	// Changing lane to left trajectory
	if (nCurrentLane >= 1 && Change_lane_to_left)
	{
		const int nTargetLane = nCurrentLane - 1;
		const double targetD = Utils::LaneNumberToD(nTargetLane);
		pool.SetOtherCars(CarsOnThisLaneTrajectories);
		pool.AddOtherCars(rSF.GetOtherCarsTrajectoryInLane(currStateX6, nTargetLane, m_HorizontPrediction, m_TimeStep));
		for (double v = 0; v < m_SpeedLimit; v += 1)
		{
			for (double T = 1; T < maxT; T += 1)
			{
				TrajectoryPtr pTraj = Trajectory::Generate_longitudinal_and_lateral_trajectory(currStateX6, targetD, v, currTime, T, LaneChangingTime);
				pool.AddTrajectory(pTraj);
			}
		}
	}

	VehicleState* pNextState = this;
	TrajectoryPtr pOptimalTraj = pool.OptimalTrajectory();
	// 目标车道
	const auto targetD = pOptimalTraj->GetTargetD();

	if (currentD != targetD)
	{
		pNextState = new LaneChanging (nCurrentLane, Utils::DtoLaneNumber(targetD));
	}
	
	return tuple<VehicleState*, TrajectoryPtr>(pNextState, pOptimalTraj);
}


